#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <tf/tf.h>

void imu_callback(sensor_msgs::Imu msgs)
{
  if (msgs.orientation_covariance[0] < 0)
        return;
        tf::Quaternion quaterion(
            msgs.orientation.x,
            msgs.orientation.y,
            msgs.orientation.z,
            msgs.orientation.w
        );

        double roll, pitch ,yaw;
        tf::Matrix3x3(quaterion).getRPY(roll, pitch, yaw);
        roll  = roll * 180 / M_PI;
        pitch = pitch * 180 / M_PI;
        yaw = yaw * 180 / M_PI;
        ROS_INFO("滚转 = %.0f   俯仰 = %.0f  朝向 = %.0f", roll, pitch, yaw);
}
int main(int argc, char* argv[])
{
 setlocale(LC_ALL, "zh_CN.utf-8");
 ros::init(argc, argv, "imu_node");
 ros::NodeHandle nh;
 ros::Subscriber sub = nh.subscribe("/imu/data", 10, &imu_callback);
ros::spin();


    return 0;
}